#include "Character.hpp"

#include <BulletSoftBody/btSoftRigidDynamicsWorld.h>
#include <BulletSoftBody\btSoftBody.h>
#include <BulletSoftBody\btSoftBodyHelpers.h>

#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreEntity.h>
#include <OGRE/OgreManualObject.h>
#include <OGRE/OgreMaterialManager.h>
#include <OGRE/OgreParticleSystem.h>
#include <OGRE/OgreDefaultHardwareBufferManager.h>
#include <OGRE/OgreMeshManager.h>
#include <OGRE/OgreMesh.h>
#include <OGRE/OgreSubMesh.h>

#include <OGRE/OgreAnimationState.h>

#include "BtOgre\BtOgreExtras.h"

#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
#include "BulletDynamics/Character/btKinematicCharacterController.h"
#include "BulletCollision/CollisionDispatch/btGhostObject.h"
#include "btBulletDynamicsCommon.h"
#include <BulletCollision\CollisionShapes\btCapsuleShape.h>

#include <algorithm>

Character::Character(Ogre::SceneManager* scene, btSoftRigidDynamicsWorld* world, btSoftBodyWorldInfo* sbworldinfo)
	: m_pScene(scene), m_pPhysicsWorld(world), m_SoftBodyWorldInfo(sbworldinfo), m_State(CS_NONE), m_Position(btVector3(0,0,0)), m_OldY(100)
{
}

void Character::createBlobForm(const BlobShapeParams& bsp, const btVector3& startPos)
{
	m_LiquidBody = btSoftBodyHelpers::CreateEllipsoid(*m_SoftBodyWorldInfo, startPos, btVector3(2,2,2), 100);

	//set the liquid body properties
	m_LiquidBody->m_cfg.kPR = bsp.kpr;
	m_LiquidBody->m_cfg.kDP = bsp.kdp;
	m_LiquidBody->m_cfg.kDF = bsp.kdf;
	m_LiquidBody->m_cfg.kKHR = 1.f; //we hardcode this parameter, since any value below 1.0 means the soft body does less than full correction on penetration
	m_LiquidBody->m_cfg.kCHR  = 1.f;
	
	m_LiquidBody->generateClusters(100);
	m_LiquidBody->m_materials[0]->m_kLST = bsp.mklst;
	/*m_pCharacter->m_LiquidBody->setCcdMotionThreshold(0.1);
	m_pCharacter->m_LiquidBody->setCcdSweptSphereRadius(3.f);*/
	m_BSP = bsp;
}

void Character::init()
{
	//manual objects are used to generate new meshes based on raw vertex data
	//this is used for the liquid form
	m_ManualObject = m_pScene->createManualObject();
	m_ManualObject->setDynamic(true);
	m_ManualObject->setCastShadows(true);

	/*
		The following code needs to be run once to setup the vertex buffer with data based on
		the bullet soft body information.
	*/
	btSoftBody::tNodeArray& nodes(m_LiquidBody->m_nodes);
	btSoftBody::tFaceArray& faces(m_LiquidBody->m_faces);

	m_ManualObject->estimateVertexCount(faces.size()*3);
	m_ManualObject->estimateIndexCount(faces.size()*3);

	m_ManualObject->begin("CharacterMaterials/LiquidBody", Ogre::RenderOperation::OT_TRIANGLE_LIST);
	for (int i = 0; i < faces.size(); i++)
	{
		btSoftBody::Node *node0 = 0, *node1 = 0, *node2 = 0;
		node0 = faces[i].m_n[0];
		node1 = faces[i].m_n[1];
		node2 = faces[i].m_n[2];
				
		m_ManualObject->position(node0->m_x[0], node0->m_x[1], node0->m_x[2]);
		m_ManualObject->normal(node0->m_n[0], node0->m_n[1], node0->m_n[2]);
				
		m_ManualObject->position(node1->m_x[0], node1->m_x[1], node1->m_x[2]);
		m_ManualObject->normal(node1->m_n[0], node1->m_n[1], node1->m_n[2]);
				
		m_ManualObject->position(node2->m_x[0], node2->m_x[1], node2->m_x[2]);
		m_ManualObject->normal(node2->m_n[0], node2->m_n[1], node2->m_n[2]);
		m_ManualObject->index(i*3);
		m_ManualObject->index(i*3+1);
		m_ManualObject->index(i*3+2);
	}
	m_ManualObject->end();



	btTransform startTransform;
	startTransform.setIdentity ();
	startTransform.setOrigin (btVector3(0.0, 4.0, 0.0));
	//startTransform.setOrigin (btVector3(10.210098,-1.6433364,16.453260));

	m_SolidGhostObject = new btPairCachingGhostObject();
	m_SolidGhostObject->setWorldTransform(startTransform);

	btScalar characterHeight=5;
	btScalar characterWidth =2;
	btConvexShape* capsule = new btCapsuleShape(characterWidth,characterHeight);
	m_SolidGhostObject->setCollisionShape (capsule);
	m_SolidGhostObject->setCollisionFlags (btCollisionObject::CF_CHARACTER_OBJECT);

	btScalar stepHeight = btScalar(3);
	m_SolidCharacterController = new btKinematicCharacterController(m_SolidGhostObject, capsule, stepHeight);
	m_SolidCharacterController->setWalkDirection(btVector3(0,0,0));

	//---Animations----
	// this is very important due to the nature of the exported animations
	m_SolidEntity->getSkeleton()->setBlendMode(Ogre::ANIMBLEND_CUMULATIVE);

	Ogre::String animNames[] = 
	{"IdleBase", "IdleTop", "RunBase", "RunTop", "HandsClosed", "HandsRelaxed", "DrawSwords",
	"SliceVertical", "SliceHorizontal", "Dance", "JumpStart", "JumpLoop", "JumpEnd"};

	// populate our animation list
	for (int i = 0; i <  NUM_ANIMS; i++)
	{
		mAnims[i] = m_SolidEntity->getAnimationState(animNames[i]);
		mAnims[i]->setLoop(true);
		mFadingIn[i] = false;
		mFadingOut[i] = false;
	}

	// start off in the idle state (top and bottom together)
	setBaseAnimation(ANIM_IDLE_BASE);
	setTopAnimation(ANIM_IDLE_TOP);

	// relax the hands since we're not holding anything
	mAnims[ANIM_HANDS_RELAXED]->setEnabled(true);

	mSwordsDrawn = false;
	//-----------------

}





void Character::changeState(int state)
{
	//first of all, let's avoid doing anything if we are already in the requested state
	if (m_State == state)
		return;

	//detach all objects, so we don't accidentally end up with two character states visible
	m_pScene->getSceneNode("CharacterNode")->detachAllObjects();
	m_pScene->getSceneNode("CharacterBlobNode")->detachAllObjects();

	btVector3 currentPos = m_Position;

	switch (m_State)
	{
		case CS_SOLID:
			currentPos = m_SolidCharacterController->getGhostObject()->getWorldTransform().getOrigin();
			m_pPhysicsWorld->removeAction(m_SolidCharacterController);
			m_pPhysicsWorld->removeCollisionObject(m_SolidGhostObject);
			/*currentPos = m_SolidBody->getWorldTransform().getOrigin();
			m_pPhysicsWorld->removeRigidBody(m_SolidBody);*/
			break;
		case CS_LIQUID:
			currentPos = calculateBlobCenter();
			m_pPhysicsWorld->removeSoftBody(m_LiquidBody);
			break;
		case CS_GAS:
			currentPos = m_GasBody->getWorldTransform().getOrigin();
			m_pPhysicsWorld->removeRigidBody(m_GasBody);
			break;
	}
	m_State = state;

	setPosition(currentPos);

	//do the correct thing based on which state we are changing into
	switch (state)
	{
		case CS_SOLID:
			m_pScene->getSceneNode("CharacterNode")->attachObject(m_SolidEntity);
			//m_pScene->getSceneNode("CharacterNode")->scale(0.1,0.1,0.1);
			m_pPhysicsWorld->addCollisionObject(m_SolidGhostObject, 4, 11);
			m_pPhysicsWorld->addAction(m_SolidCharacterController);
			/*
			m_pScene->getSceneNode("CharacterNode")->attachObject(m_SolidEntity);
			m_pPhysicsWorld->addRigidBody(m_SolidBody, 4, 11);
			*/
			break;
		case CS_LIQUID:
			//interestingly, we don't do anything for the liquid form here
			//because setPosition() already does these two steps
			/*m_pScene->getSceneNode("CharacterBlobNode")->attachObject(m_ManualObject);
			m_pPhysicsWorld->addSoftBody(m_LiquidBody, 4, 11);*/
			
			m_pScene->getSceneNode("CharacterBlobNode")->attachObject(m_ManualObject);
			break;
		case CS_GAS:
			m_pScene->getSceneNode("CharacterNode")->attachObject(m_GasPS);
			m_pPhysicsWorld->addRigidBody(m_GasBody, 4, 11);
			m_GasBody->setGravity(btVector3(0,5,0));
			m_GasBody->setActivationState(DISABLE_DEACTIVATION);
			break;
	}
}


void Character::applyForce(float x, float y, float z)
{

	switch (m_State)
	{
		case CS_SOLID:

			m_SolidCharacterController->setWalkDirection(btVector3(x, y, z));
			//m_SolidBody->applyCentralForce(btVector3(x, y, z));
			break;
		case CS_LIQUID:
			m_LiquidBody->addForce(btVector3(x, y, z));
			m_LiquidBody->applyForces();
			break;
		case CS_GAS:
			m_GasBody->applyCentralForce(btVector3(x, y, z));
			break;
	}
}


void Character::update(float dt, Ogre::SceneNode* m_pCameraNode, Ogre::Vector3 forwardVector)
{
	btVector3 currentPos;
	char msg[200];

	switch (m_State)
	{
		case CS_SOLID:
			{
				dt = dt/1000;
				currentPos = m_SolidCharacterController->getGhostObject()->getWorldTransform().getOrigin();

				Ogre::Vector3 mGoalDirection = Ogre::Vector3::ZERO;   // we will calculate this

				if (forwardVector != Ogre::Vector3::ZERO)
				{
					if (mBaseAnimID == ANIM_IDLE_BASE)
					{
						setBaseAnimation(ANIM_RUN_BASE, true);
						if (mTopAnimID == ANIM_IDLE_TOP) setTopAnimation(ANIM_RUN_TOP, true);
					}
					// calculate actually goal direction in world based on player's key directions
					mGoalDirection = forwardVector;
					mGoalDirection.normalise();
					Ogre::Quaternion toGoal = m_pScene->getSceneNode("CharacterNode")->getOrientation().zAxis().getRotationTo(mGoalDirection);

					// calculate how much the character has to turn to face goal direction
					float yawToGoal = toGoal.getYaw().valueDegrees();
					// this is how much the character CAN turn this frame
					float yawAtSpeed = yawToGoal / Ogre::Math::Abs(yawToGoal) * dt * TURN_SPEED;
					// reduce "turnability" if we're in midair
					if (mBaseAnimID == ANIM_JUMP_LOOP) yawAtSpeed *= 0.2f;

					// turn as much as we can, but not more than we need to
					if (yawToGoal < 0) yawToGoal = std::min<float>(0, std::max<float>(yawToGoal, yawAtSpeed)); //yawToGoal = Math::Clamp<Real>(yawToGoal, yawAtSpeed, 0);
					else if (yawToGoal > 0) yawToGoal = std::max<float>(0, std::min<float>(yawToGoal, yawAtSpeed)); //yawToGoal = Math::Clamp<Real>(yawToGoal, 0, yawAtSpeed);

					m_pScene->getSceneNode("CharacterNode")->yaw(Ogre::Degree(yawToGoal));

				} else
				{
					if (mBaseAnimID == ANIM_RUN_BASE)
					{
						// stop running if already moving and the player doesn't want to move
						setBaseAnimation(ANIM_IDLE_BASE);
						if (mTopAnimID == ANIM_RUN_TOP) setTopAnimation(ANIM_IDLE_TOP);
					}
				}

				//Animations!!!

				float baseAnimSpeed = 1;
				float topAnimSpeed = 1;

				mTimer += dt;

				// increment the current base and top animation times
				if (mBaseAnimID != ANIM_NONE) mAnims[mBaseAnimID]->addTime(dt * baseAnimSpeed);
				if (mTopAnimID != ANIM_NONE) mAnims[mTopAnimID]->addTime(dt * topAnimSpeed);

				// apply smooth transitioning between our animations
				fadeAnimations(dt);
				break;
			}
		case CS_LIQUID:
		{
			//grab the calculated mesh data from the physics body
			btSoftBody::tNodeArray& nodes(m_LiquidBody->m_nodes);
			btSoftBody::tFaceArray& faces(m_LiquidBody->m_faces);

			float minx, miny, minz, maxx, maxy, maxz;
			minx = maxx = nodes[0].m_x[0];
			miny = maxy = nodes[0].m_x[1];
			minz = maxz = nodes[0].m_x[2];

			m_ManualObject->beginUpdate(0);
			for (int i = 0; i < faces.size(); i++)
			{
				btSoftBody::Node *node0 = 0, *node1 = 0, *node2 = 0;
				node0 = faces[i].m_n[0];
				node1 = faces[i].m_n[1];
				node2 = faces[i].m_n[2];
				
				m_ManualObject->position(node0->m_x[0], node0->m_x[1], node0->m_x[2]);
				m_ManualObject->normal(node0->m_n[0], node0->m_n[1], node0->m_n[2]);
				
				m_ManualObject->position(node1->m_x[0], node1->m_x[1], node1->m_x[2]);
				m_ManualObject->normal(node1->m_n[0], node1->m_n[1], node1->m_n[2]);
				
				m_ManualObject->position(node2->m_x[0], node2->m_x[1], node2->m_x[2]);
				m_ManualObject->normal(node2->m_n[0], node2->m_n[1], node2->m_n[2]);
				m_ManualObject->index(i*3);
				m_ManualObject->index(i*3+1);
				m_ManualObject->index(i*3+2);
			}
			m_ManualObject->end();

			/*Ogre::MeshPtr mesh = m_BlobEntity->getMesh();
			m_ManualObject->convertToMesh("MyMesh");*/
			currentPos = calculateBlobCenter();
			break;
		}
		case CS_GAS:
			currentPos = m_GasBody->getWorldTransform().getOrigin();
			break;
	}

	/*sprintf(msg, "\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n", 
		matrix[0], matrix[1], matrix[2], matrix[3],
		matrix[4] ,matrix[5],matrix[6],matrix[7],
		matrix[8],matrix[9],matrix[10],matrix[11],
		matrix[12],matrix[13],matrix[14],matrix[15]);
	Ogre::LogManager::getSingletonPtr()->logMessage(msg);*/
	if (m_State != CS_NONE)
	{
		m_pScene->getSceneNode("CharacterNode")->setPosition(currentPos.x(), currentPos.y(), currentPos.z());
		
	}
}


void Character::fadeAnimations(float dt)
{
	for (int i = 0; i < NUM_ANIMS; i++)
	{
		if (mFadingIn[i])
		{
			// slowly fade this animation in until it has full weight
			float newWeight = mAnims[i]->getWeight() + dt * ANIM_FADE_SPEED;
			mAnims[i]->setWeight(Ogre::Math::Clamp<float>(newWeight, 0, 1));
			if (newWeight >= 1) mFadingIn[i] = false;
		}
		else if (mFadingOut[i])
		{
			// slowly fade this animation out until it has no weight, and then disable it
			float newWeight = mAnims[i]->getWeight() - dt * ANIM_FADE_SPEED;
			mAnims[i]->setWeight(Ogre::Math::Clamp<float>(newWeight, 0, 1));
			if (newWeight <= 0)
			{
				mAnims[i]->setEnabled(false);
				mFadingOut[i] = false;
			}
		}
	}
}


bool Character::isFalling()
{
	char msg[200];
	switch (m_State)
	{
		case CS_SOLID:
		{
			/*sprintf(msg, "SOLID Y velocity: %f and diff: %f", m_SolidGhostObject->getWorldTransform().getOrigin().y(), m_SolidGhostObject->getWorldTransform().getOrigin().y() - m_OldY);
			Ogre::LogManager::getSingleton().logMessage(msg);*/
			double y = m_SolidGhostObject->getWorldTransform().getOrigin().y();
			if (y - m_OldY < -0.1)
			{
				m_OldY = y;
				return true;
			}
			break;
		}
		case CS_LIQUID:
		{
			btVector3 vec = calculateBlobCenter();

			/*sprintf(msg, "BLOB Y: %f oldY: %f diff: %f", vec.y(), m_OldY, vec.y() - m_OldY);
			Ogre::LogManager::getSingleton().logMessage(msg);*/

			if ((vec.y() - m_OldY) < 0)
			{
				m_OldY = vec.y();
				return true;
			}
			break;
		}
		case CS_GAS:
			return false;
			break;
	}

	return false;
}

void Character::setPosition(const btVector3& pos)
{
	btVector3 currentPos;
	m_Position = pos;
	btVector3 vecmin, vecmax;
	double yoffset = 0;
	/*
		In all three states we calculate an offset based on the body's
		aabb (Axis aligned bounding box). This is to counter the effect
		that changing state also changes the vertical position of the character.
	*/
	switch (m_State)
	{
		case CS_SOLID:
			m_SolidCharacterController->getGhostObject()->getCollisionShape()->getAabb(m_SolidCharacterController->getGhostObject()->getWorldTransform(),vecmin, vecmax);
			yoffset = (vecmax.y() - vecmin.y()) / 3;
			m_Position.setY( m_Position.y() + yoffset);

			//m_character->getGhostObject()->getWorldTransform().setOrigin(m_Position);
			m_SolidCharacterController->warp(m_Position);
			break;
		case CS_LIQUID:
			{
			/*m_LiquidBody->getAabb(vecmin, vecmax);
			yoffset = vecmax.y() - vecmin.y();
			if (m_Position.y()  > yoffset)
				m_Position.setY( m_Position.y() - (yoffset / 3));
			else
				m_Position.setY( m_Position.y() + (yoffset / 3));*/
			
			if (m_LiquidBody->isActive())
				m_pPhysicsWorld->removeSoftBody(m_LiquidBody);
			delete m_LiquidBody;
			createBlobForm(m_BSP, m_Position);
			btSoftBody::tNodeArray& nodes(m_LiquidBody->m_nodes);
			btSoftBody::tFaceArray& faces(m_LiquidBody->m_faces);

			float minx, miny, minz, maxx, maxy, maxz;
			minx = maxx = nodes[0].m_x[0];
			miny = maxy = nodes[0].m_x[1];
			minz = maxz = nodes[0].m_x[2];

			m_ManualObject->beginUpdate(0);
			for (int i = 0; i < faces.size(); i++)
			{
				btSoftBody::Node *node0 = 0, *node1 = 0, *node2 = 0;
				node0 = faces[i].m_n[0];
				node1 = faces[i].m_n[1];
				node2 = faces[i].m_n[2];
				
				m_ManualObject->position(node0->m_x[0], node0->m_x[1], node0->m_x[2]);
				m_ManualObject->normal(node0->m_n[0], node0->m_n[1], node0->m_n[2]);
				
				m_ManualObject->position(node1->m_x[0], node1->m_x[1], node1->m_x[2]);
				m_ManualObject->normal(node1->m_n[0], node1->m_n[1], node1->m_n[2]);
				
				m_ManualObject->position(node2->m_x[0], node2->m_x[1], node2->m_x[2]);
				m_ManualObject->normal(node2->m_n[0], node2->m_n[1], node2->m_n[2]);
				m_ManualObject->index(i*3);
				m_ManualObject->index(i*3+1);
				m_ManualObject->index(i*3+2);
			}
			m_ManualObject->end();

			/*Ogre::MeshPtr mesh = m_BlobEntity->getMesh();
			m_ManualObject->convertToMesh("MyMesh");*/

			currentPos = calculateBlobCenter();
			if (m_State != CS_NONE)
			{
				m_pScene->getSceneNode("CharacterNode")->setPosition(currentPos.x(), currentPos.y(), currentPos.z());	
			}
			m_pPhysicsWorld->addSoftBody(m_LiquidBody, 4, 11);
			break;
			}
		case CS_GAS:
			m_GasBody->getAabb(vecmin, vecmax);
			yoffset = (vecmax.y() - vecmin.y()) / 3;
			m_Position.setY( m_Position.y() + yoffset);

			m_GasBody->getWorldTransform().setOrigin(m_Position);
			break;
	}

}

btVector3 Character::calculateBlobCenter()
{
	btSoftBody::tNodeArray& nodes(m_LiquidBody->m_nodes);
	btVector3 pos(0,0,0);
	for (int i = 0; i < nodes.size(); i++)
	{
		pos = pos + nodes[i].m_x;
	}
	pos = pos / (double)nodes.size();
	return pos;
}